/**
* @author: Jinghui Wang
*/

#include <pointcloud_to_grid.hpp>

namespace p2g {

PointCloudToGridNode::PointCloudToGridNode(const rclcpp::NodeOptions & options)
    : rclcpp::Node("pointcloud_to_grid", options) {

    input_topic_ = this->declare_parameter("input_topic", "/cloud");
    output_topic_ = this->declare_parameter("output_topic", "/grid");
    output_frame_ = this->declare_parameter("output_frame", "");
    min_height_ = this->declare_parameter("min_height", std::numeric_limits<double>::lowest());
    max_height_ = this->declare_parameter("max_height", std::numeric_limits<double>::max());
    map_resolution_ = this->declare_parameter("map_resolution", 0.1);
    map_origin_x_ = this->declare_parameter("map_origin_x", std::numeric_limits<double>::infinity());
    map_origin_y_ = this->declare_parameter("map_origin_y", std::numeric_limits<double>::infinity());
    update_period_ = this->declare_parameter("update_period", 3.0);

    sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        input_topic_, 10, std::bind(&PointCloudToGridNode::cloudCallBack, this, std::placeholders::_1));

    pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(output_topic_, 10);

    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(static_cast<int>(update_period_ * 1000)),
        std::bind(&PointCloudToGridNode::timerCallBack, this)
    );
}

void PointCloudToGridNode::cloudCallBack(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    latest_cloud_mag_ = msg;
}

void PointCloudToGridNode::timerCallBack() {

    if( latest_cloud_mag_ == nullptr ) {
        RCLCPP_WARN(this->get_logger(), "No latest point cloud received yet.");
        return;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*latest_cloud_mag_, *cloud);

    // auto start = std::chrono::high_resolution_clock::now();

    // Downsample
    pcl::UniformSampling<pcl::PointXYZ> uniform_sampling;
    uniform_sampling.setInputCloud(cloud);
    uniform_sampling.setRadiusSearch(map_resolution_);
    pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    uniform_sampling.filter(*downsampled_cloud);

    // Filter by height
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(downsampled_cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(min_height_, max_height_);
    pass.filter(*filtered_cloud);

    // Convert to occupancy grid
    pcl::PointXYZ min_pt, max_pt;
    pcl::getMinMax3D(*filtered_cloud, min_pt, max_pt);

    nav_msgs::msg::OccupancyGrid grid;
    grid.header = latest_cloud_mag_->header;

    // Deal with frame id
    if (output_frame_.empty()) {
        output_frame_ = latest_cloud_mag_->header.frame_id;
    }
    else {
        grid.header.frame_id = output_frame_;
    }

    // Default: make grid map to fit with pointcloud
    if( map_origin_x_ == std::numeric_limits<double>::infinity() ) {
        grid.info.origin.position.x = min_pt.x;
    }
    else {
        grid.info.origin.position.x = map_origin_x_;
    }

    if( map_origin_y_ == std::numeric_limits<double>::infinity() ) {
        grid.info.origin.position.y = min_pt.y;
    }
    else {
        grid.info.origin.position.y = map_origin_y_;
    }

    // Initialize the grid map
    grid.info.resolution = map_resolution_;
    grid.info.width = std::ceil((max_pt.x - min_pt.x) / map_resolution_);
    grid.info.height = std::ceil((max_pt.y - min_pt.y) / map_resolution_);
    grid.data.resize(grid.info.width * grid.info.height, 0); // probabilities are in the range [0,100].  Unknown is -1.

    for (const auto &point : filtered_cloud->points) {
        int x = std::floor((point.x - min_pt.x) / map_resolution_);
        int y = std::floor((point.y - min_pt.y) / map_resolution_);
        if (x >= 0 && x < grid.info.width && y >= 0 && y < grid.info.height) {
            grid.data[y * grid.info.width + x] = 100;
        }
    }

    // auto end = std::chrono::high_resolution_clock::now();
    // auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
    // RCLCPP_DEBUG(this->get_logger(), "PointCloud to Grid conversion took %ld us", duration.count());

    // publish the grid
    grid.header.stamp = this->now();
    pub_->publish(grid);

    latest_cloud_mag_.reset();
}



} // namespace p2g
